﻿//////////////////////////////////////////////////////////////////////////////////////////////////////////
//		            Kinect
//////////////////////////////////////////////////////////////////////////////////////////////////////////
#include "windows.h"
#include "NuiApi.h"
#include "CKAll.h"
//#include "kinectTest.h"
#pragma warning(disable:4430)

CKObjectDeclaration	*FillBehaviorKinectDecl();
CKERROR CreateKinectProto(CKBehaviorPrototype **);
int Kinect(const CKBehaviorContext& BehContext);
int KinectCallBack(const CKBehaviorContext& BehContext);

//kinect knct;
HRESULT		hr = NULL;
HANDLE		m_pDepthStreamHandle = NULL;
HANDLE		m_pVideoStreamHandle = NULL;
DWORD		dwFlags =	NUI_INITIALIZE_FLAG_USES_SKELETON|
						NUI_INITIALIZE_FLAG_USES_COLOR| 
						NUI_INITIALIZE_FLAG_USES_DEPTH;
const NUI_IMAGE_FRAME *imageVideoFrame = NULL;
const NUI_IMAGE_FRAME *imageDepthFrame = NULL;

DWORD		m_SkeletonIds[NUI_SKELETON_COUNT];
VxVector	vct[NUI_SKELETON_COUNT][NUI_SKELETON_POSITION_COUNT];
bool		bKinectStartup = false;
CKDataArray *pPositionArray;
CKDataArray *pSkeletalArray;

CKObjectDeclaration	*FillBehaviorKinectDecl()
{
	CKObjectDeclaration *od = CreateCKObjectDeclaration("Kinect");	
	
	od->SetType(CKDLL_BEHAVIORPROTOTYPE);
	od->SetVersion(0x00000001);
	od->SetCreationFunction(CreateKinectProto);
	od->SetDescription("kinect test");
	od->SetCategory("Kinect");
	od->SetGuid(CKGUID(0x875a86ec, 0x0e9825bc));
	od->SetAuthorGuid(CKGUID(0x56495254,0x4f4f4c53));
	od->SetAuthorName("Virtools");
	od->SetCompatibleClassId(CKCID_BEOBJECT);

	return od;
}

CKERROR CreateKinectProto(CKBehaviorPrototype** pproto)
{
	CKBehaviorPrototype *proto = CreateCKBehaviorPrototype("Kinect");
	if (!proto) {
		return CKERR_OUTOFMEMORY;
	}

//---     Inputs declaration
	proto->DeclareInput("In0");

//---     Outputs declaration
	proto->DeclareOutput("Out0");

	proto->DeclareInParameter("VideoTexture",CKPGUID_TEXTURE);
	proto->DeclareInParameter("DepthTexture",CKPGUID_TEXTURE);
	proto->DeclareInParameter("PositionArray",CKPGUID_DATAARRAY);
	proto->DeclareInParameter("SkeletalArray",CKPGUID_DATAARRAY);
	proto->DeclareInParameter("ElevationAngle",CKPGUID_INT,"0");

	proto->DeclareOutParameter("success",CKPGUID_STRING);
	proto->DeclareOutParameter("SkeletonID",CKPGUID_INT);
//----	Local Parameters Declaration

//----	Settings Declaration
	proto->DeclareSetting("filter",CKPGUID_FLOAT,"1");
	proto->DeclareLocalParameter("AngleOld",CKPGUID_INT,"0");

	proto->SetBehaviorCallbackFct(KinectCallBack,CKCB_BEHAVIORCREATE|CKCB_BEHAVIORATTACH|CKCB_BEHAVIORDETACH|CKCB_BEHAVIORDELETE|CKCB_BEHAVIOREDITED|CKCB_BEHAVIORSETTINGSEDITED|CKCB_BEHAVIORLOAD|CKCB_BEHAVIORPRESAVE|CKCB_BEHAVIORPOSTSAVE|CKCB_BEHAVIORRESUME|CKCB_BEHAVIORPAUSE|CKCB_BEHAVIORRESET|CKCB_BEHAVIORRESET|CKCB_BEHAVIORDEACTIVATESCRIPT|CKCB_BEHAVIORACTIVATESCRIPT|CKCB_BEHAVIORREADSTATE, NULL);
	proto->SetFunction(Kinect);

	*pproto = proto;
	return CK_OK;
}

int Kinect(const CKBehaviorContext& BehContext)
{
	CKBehavior *beh = BehContext.Behavior;
	CKContext *ctx = beh->GetCKContext();
	
	NUI_SKELETON_FRAME SkeletonFrame = {0};
	int angle = 0;

	//NUI_IMAGE_FRAME* imageFrame = new NUI_IMAGE_FRAME;

	if(beh->IsInputActive(0))
	{
		beh->ActivateInput(0,FALSE);
		if(bKinectStartup)
		{
			CKSTRING pcstr = "已经有kinect实例运行";
			beh->SetOutputParameterValue(0,pcstr);
			return CKBR_GENERICERROR;
		}
		hr = NuiInitialize(dwFlags);
		if(FAILED(hr))
		{
			CKSTRING pcstr = "kinect初始化失败";
			beh->SetOutputParameterValue(0,pcstr);
			return CKBR_GENERICERROR;
		}

		//elevation angle of the Kinect sensor
		//if(CK_OK == beh->GetInputParameterValue(3,&angle))
		//{
		//	if( NUI_CAMERA_ELEVATION_MINIMUM <= angle <=NUI_CAMERA_ELEVATION_MAXIMUM)
		//	{
		//		hr = NuiCameraElevationSetAngle((LONG)angle);
		//		//if(FAILED(hr))
		//		//{
		//		//	CKSTRING pcstr = "kinect朝向调整失败";
		//		//	beh->SetOutputParameterValue(0,pcstr);
		//		//	ctx->OutputToConsole("kinect朝向调整失败");
		//		//}
		//		//else
		//		//{
		//		//	CKSTRING pcstr = "kinect朝向调整成功";
		//		//	beh->SetOutputParameterValue(0,pcstr);
		//		beh->SetLocalParameterValue(1,&angle);
		//		//	ctx->OutputToConsole("kinect朝向调整成功");
		//		//}
		//	}
		//}	


		//imageStream初始化视频图像流
		
		hr = NuiImageStreamOpen(NUI_IMAGE_TYPE_COLOR,
								NUI_IMAGE_RESOLUTION_640x480,
								0,2,
								NULL,
								&m_pVideoStreamHandle);
		if(FAILED(hr))
		{
			CKSTRING pcstr = "NuiImageStreamOpen error!";
			beh->SetOutputParameterValue(0,pcstr);
			return CKBR_GENERICERROR;
		}
		
		// 初始化深度图像流
		hr = NuiImageStreamOpen(NUI_IMAGE_TYPE_DEPTH,
								NUI_IMAGE_RESOLUTION_320x240,
								0,2,
								NULL,
								&m_pDepthStreamHandle);
		if(FAILED(hr))
		{
			CKSTRING pcstr = "NuiDepthStreamOpen error!";
			beh->SetOutputParameterValue(0,pcstr);
			return CKBR_GENERICERROR;
		}
		
		if(pPositionArray = (CKDataArray*) beh->GetInputParameterObject(2))
		{
			while(pPositionArray->GetRowCount() < NUI_SKELETON_COUNT)
			pPositionArray->AddRow();
		}
		

		if(pSkeletalArray = (CKDataArray*) beh->GetInputParameterObject(3))
		{}

		ZeroMemory(m_SkeletonIds,sizeof(m_SkeletonIds));
		beh->ActivateOutput(0);
		CKSTRING pcstr = "kinect初始化成功";
		bKinectStartup = true;
		beh->SetOutputParameterValue(0,pcstr);
		return CKBR_ACTIVATENEXTFRAME;
	}// Active end!
	
	////elevation angle 
	if(CK_OK == beh->GetInputParameterValue(4,&angle))
	{
		int oldAngle = -28;
		if(CK_OK ==beh->GetLocalParameterValue(1,&oldAngle))
		{
			if((angle!=oldAngle) && (NUI_CAMERA_ELEVATION_MINIMUM < angle) &&(angle < NUI_CAMERA_ELEVATION_MAXIMUM))
			{
				hr = NuiCameraElevationSetAngle((LONG)angle);
				beh->SetLocalParameterValue(1,&angle);
			}
		}
	}

	//// imageVideoStream
	CKTexture *pVideoTex = (CKTexture *)beh->GetInputParameterObject(0);
	if (pVideoTex)
	{
		hr = NuiImageStreamGetNextFrame( m_pVideoStreamHandle, 0,&imageVideoFrame );
		if(SUCCEEDED(hr))
		{
			//锁定virtools图片pTex
			CKBYTE *data = pVideoTex->LockSurfacePtr();

			//锁定INuiFrame
			INuiFrameTexture *pVideoTexture = (imageVideoFrame)->pFrameTexture;
			NUI_LOCKED_RECT LockedRect;
			pVideoTexture->LockRect( 0, &LockedRect, NULL, 0 );
			pVideoTex->ResizeImages(640,480);
			if ( LockedRect.Pitch != 0 )
			{
				//ctx->OutputToConsole("lockRect");
				//int w = pTex->GetWidth();
				//int h = pTex->GetHeight();
				//ctx->OutputToConsoleEx("w:%i h:%i",w,h);
				
				// copy image data				
				for (int i = 0; i<LockedRect.size;i++)
				{
					data[i] = LockedRect.pBits[i]; 
				}
				//for (int y=0; y<480; y++)
				//{
				//	for (int x=0; x<640; x++)
				//	{
				//		data[x+y*480] = 0;
				//		int m = x+y*480;
				//		ctx->OutputToConsoleEx("data[%i]:%i",m,data[m]);
				//		for(int z=0; z<4; z++)
				//		{
				//			data[m]+=LockedRect.pBits[m];
				//			ctx->OutputToConsole("复值成功");
				//			data[m]<<=8;
				//			ctx->OutputToConsoleEx("左移8位：%i",data[m]);
				//		}
				//	}
				//}
				//ctx->OutputToConsole("copy data");
			}
			else
			{
				CKSTRING pcstr = "Buffer length of received texture is bogus!";
				beh->SetOutputParameterValue(0,pcstr);
			}// end if (LockedRect.Pitch != 0 )

			pVideoTexture->UnlockRect( 0 );//解锁INuiFrame
			pVideoTex->ReleaseSurfacePtr();//解锁virtools pTex
		}
		NuiImageStreamReleaseFrame( m_pVideoStreamHandle, imageVideoFrame);
	}

	//// imageDepthStream
	CKTexture *pDepthTex = (CKTexture *)beh->GetInputParameterObject(1);
	if (pDepthTex)
	{
		hr = NuiImageStreamGetNextFrame( m_pDepthStreamHandle, 0,&imageDepthFrame );
		if(SUCCEEDED(hr))
		{
			//锁定virtools图片pTex
			CKBYTE *data = pDepthTex->LockSurfacePtr();

			//锁定INuiFrame
			INuiFrameTexture *pDepthTexture = (imageDepthFrame)->pFrameTexture;
			NUI_LOCKED_RECT LockedRect;
			pDepthTexture->LockRect( 0, &LockedRect, NULL, 0 );
			//ctx->OutputToConsoleEx("%i,%i",LockedRect.Pitch,LockedRect.size/LockedRect.Pitch);
			pDepthTex->ResizeImages(320,240);

			if ( LockedRect.Pitch != 0 )
			{
				for (int i = 0; i<LockedRect.size;i++)
				{
					data[i] = LockedRect.pBits[i]; 
				}
			}
			else
			{
				CKSTRING pcstr = "Buffer length of received texture is bogus!";
				beh->SetOutputParameterValue(0,pcstr);
			}// end if (LockedRect.Pitch != 0 )

			pDepthTexture->UnlockRect( 0 );//解锁INuiFrame
			pDepthTex->ReleaseSurfacePtr();//解锁virtools pTex
		}
		NuiImageStreamReleaseFrame( m_pDepthStreamHandle, imageDepthFrame);
	}

	//SkeletonStream
	BOOL bFoundSkeleton = FALSE;
	if(SUCCEEDED(NuiSkeletonGetNextFrame(0,&SkeletonFrame)))
	{
		for ( int i = 0 ; i < NUI_SKELETON_COUNT ; i++ )
        {
            if( SkeletonFrame.SkeletonData[i].eTrackingState == NUI_SKELETON_TRACKED ||
                SkeletonFrame.SkeletonData[i].eTrackingState == NUI_SKELETON_POSITION_ONLY)
            {
                bFoundSkeleton = TRUE;
				break;
            }
        }
	}

	if(!bFoundSkeleton)
	{
		//CKSTRING pcstr = "kinect未检测到角色或角色未移动";
 		return CKBR_ACTIVATENEXTFRAME;
	}

	HRESULT hr = NuiTransformSmooth(&SkeletonFrame,NULL);
    if ( FAILED(hr) )
    {
		CKSTRING pcstr = "kinect骨骼平滑失败";
		beh->SetOutputParameterValue(0,pcstr);
        return CKBR_ACTIVATENEXTFRAME;
    }
	
	bool bSkeletonIdsChanged = false;
	int iCurrentSkeleton = -1;
	int iChangedSkeleton = -1;
    for ( int i = 0 ; i < NUI_SKELETON_COUNT; i++ )
    {
        if ( m_SkeletonIds[i] != SkeletonFrame.SkeletonData[i].dwTrackingID )
        {
            m_SkeletonIds[i] = SkeletonFrame.SkeletonData[i].dwTrackingID;
            bSkeletonIdsChanged = true;
			iChangedSkeleton = i;
        }
		//↓2012-08-20 郭健 增加以下内容kinect1.5版后增加骨骼朝向.
		const NUI_SKELETON_DATA & skeleton = SkeletonFrame.SkeletonData[i];
		NUI_SKELETON_BONE_ORIENTATION boneOrientations[NUI_SKELETON_POSITION_COUNT];
		NuiSkeletonCalculateBoneOrientations(&skeleton, boneOrientations);
		// ↑2012-08-20 end
        // Show skeleton only if it is tracked, and the center-shoulder joint is at least inferred.
		if (SkeletonFrame.SkeletonData[i].eTrackingState == NUI_SKELETON_TRACKED &&
            SkeletonFrame.SkeletonData[i].eSkeletonPositionTrackingState[NUI_SKELETON_POSITION_SHOULDER_CENTER] != NUI_SKELETON_POSITION_NOT_TRACKED)
		{
			for(int j = 0; j < NUI_SKELETON_POSITION_COUNT; j++)
			{
				//郭健 0820
				NUI_SKELETON_BONE_ORIENTATION & orientation = boneOrientations[j];
				int startJoint = orientation.startJoint;
				int endJoint = orientation.endJoint;
				Matrix4 matrx = orientation.hierarchicalRotation.rotationMatrix;
				float vmtx[4][4];
				for(int m = 0; m < 4; m++)
				{
					for(int n = 0; n < 4; n++)
					{
						// 转换矩阵格式;
						vmtx[m][n] = *(float*)((long)&matrx + sizeof(float)*(m + n));
					}
				}
				VxMatrix mtx(vmtx);
				CK3dEntity *obj = (CK3dEntity *)pSkeletalArray->GetElementObject(j,0);
				obj->SetLocalMatrix(mtx);
				// 0820 end
				vct[i][j].x =  SkeletonFrame.SkeletonData[i].SkeletonPositions[j].x;
				vct[i][j].y =  SkeletonFrame.SkeletonData[i].SkeletonPositions[j].y;
				vct[i][j].z =  SkeletonFrame.SkeletonData[i].SkeletonPositions[j].z;
				//iCurrentSkeleton = i;
				//beh->SetOutputParameterValue(1,&iCurrentSkeleton);
				pPositionArray->SetElementValue(i, j, &vct[i][j]);
			}
		}else if (SkeletonFrame.SkeletonData[i].eTrackingState == NUI_SKELETON_POSITION_ONLY )
		{
			vct[i][0].x =  SkeletonFrame.SkeletonData[i].Position.x;
			vct[i][0].y =  SkeletonFrame.SkeletonData[i].Position.y;
			vct[i][0].z =  SkeletonFrame.SkeletonData[i].Position.z;
			pPositionArray->SetElementValue(i,0,&vct[i][0]);
		}
		//break;
	}

	VxVector tmp;
	float fLocalParameter;
	CKParameter *pLocal =(CKParameter*)beh->GetLocalParameter(0);
	pLocal->GetValue(&fLocalParameter);
	//ctx->OutputToConsoleEx("%f",fLocalParameter);
	if(0.0005 >=fLocalParameter)
	{
		//防止为零
		fLocalParameter = 1;
	}
	for(int j = 0;j<NUI_SKELETON_POSITION_COUNT;j++)
	{
		tmp.Set( vct[iCurrentSkeleton][j].x * fLocalParameter,
			vct[iCurrentSkeleton][j].y * fLocalParameter,vct[iCurrentSkeleton][j].z);
		//ctx->OutputToConsoleEx("tmp(%f,%f,%f)",tmp.x,tmp.y,tmp.z);
		beh->SetOutputParameterValue(j+2,&tmp);
	}

	//beh->ActivateOutput(0);
	return CKBR_ACTIVATENEXTFRAME;
}

int KinectCallBack(const CKBehaviorContext& BehContext)
{
	CKBehavior *beh = BehContext.Behavior;
	CKContext *ctx = beh->GetCKContext();
	switch (BehContext.CallbackMessage)
	{
		case CKM_BEHAVIORCREATE:
			char str[16];
			for(int i = 2; i <= NUI_SKELETON_POSITION_COUNT + 1;i++)
			{
				sprintf(str,"pos %d",i-2);
				beh->CreateOutputParameter(str,CKPGUID_VECTOR);
			}
			//ctx->OutputToConsole("创建输出参数完成");
			break;
		case CKM_BEHAVIORATTACH:
			break;
		case CKM_BEHAVIORDETACH:
			break;
		case CKM_BEHAVIORDELETE:
			if(bKinectStartup)
			{
				NuiShutdown();
				bKinectStartup = false;
				ctx->OutputToConsole("NuiShutdown");
			}
			break;
		case CKM_BEHAVIOREDITED:
			break;
		case CKM_BEHAVIORSETTINGSEDITED:
			break;
		case CKM_BEHAVIORLOAD:
			break;
		case CKM_BEHAVIORPRESAVE:
			break;
		case CKM_BEHAVIORPOSTSAVE:
			break;
		case CKM_BEHAVIORRESUME:
			break;
		case CKM_BEHAVIORPAUSE:
			break;
		case CKM_BEHAVIORRESET:
			if(bKinectStartup)
			{
				NuiShutdown();
				bKinectStartup = false;
				ctx->OutputToConsole("NuiShutdown");
			}			
			break;
		case CKM_BEHAVIORNEWSCENE:
			break;
		case CKM_BEHAVIORDEACTIVATESCRIPT:
			break;
		case CKM_BEHAVIORACTIVATESCRIPT:
			break;
		case CKM_BEHAVIORREADSTATE:
			break;

	}
	return CKBR_OK;
}
